
#include <iostream>
#include <Eigen/Dense>
#include <math.h>

#include "ArmDimensions.h"
#include "KinematicModelTypes.h"

dirKinResult dirKin(float teta1, float teta2, float teta3, float teta4);

invKinResult invKin(float x, float y, float z);

invKinResult invKin(float x, float y, float z, bool recursive){

	invKinResult res;

	res.reachable = false;

	float d0 = SHOULDERZ;
	float d1 = SHOULDERX;
	float a0 = SHOULDERY;
	float a2 = FIRST_LINK_LENGTH;
	float a3 = SECOND_LINK_LENGTH + HAND_CENTER_OFFSET;

	float w = sqrt( (x-d1)*(x-d1) + (y-a0)*(y-a0) + (z-d0)*(z-d0) );

	float tetaw = M_PI/2.0+atan2(z-d0, y-a0);
	float tetawd = tetaw*180.0/M_PI;

	float ctetak = (a3*a3 - (w*w) - (a2*a2)) / (-2.0*w*a2);
	float stetak = sqrt(1 - (ctetak*ctetak));
	float tetak = atan2(stetak, ctetak);
	float tetakd = tetak*180.0/M_PI;

	float teta1 = tetaw - tetak;
	float teta1d = teta1*180/M_PI;

	float teta2 = atan2(x-d1, sqrt(((z-d0)*(z-d0)) + ((y-a0)*(y-a0))));
	float teta2d = teta2*180.0/M_PI;

	/*teta3 = pi-acos(((x-d1)^2+y^2+(z-d0)^2-(a2^2)-(a3^2))/-(2*a2*a3));
	steta3 = a3*sin(teta3);
	cteta3k = a2+(a3*cos(teta3));*/

	float cteta3 = ((w*ctetak)-a2)/a3;
	float steta3 = sqrt(1 - (cteta3*cteta3));
	float teta3 = atan2(steta3, cteta3);
	float teta3d = teta3*180/M_PI;

	/*%cteta2 = ((sqrt(x^2+y^2)*(a2+a3*cteta3))+z*a3*steta3)/(a2^2+a3^2+2*a3*cteta3)
	%steta2 = (z*(a2+a3*cteta3)+(sqrt(x^2+y^2)*a3*steta3))/(a2^2+a3^2+2*a3*cteta3)
	%teta2 = atan2(steta2, cteta2)
	%teta2d = teta2*180/pi*/



	//Vincoli (per ora quelli del corpo umano)
	if(teta1d >= -30 && teta1d <= 180   &&   teta2d >= 0 && teta2d <= 180   &&   teta3d >= 0 && teta3d <= 130   &&   w <= a2+a3){

		/*//Truncating float digits
		char sz[64];
		sprintf(sz, "%.4lf\n", teta1d); 
		teta1d = atof(sz);
		sprintf(sz, "%.4lf\n", teta2d); 
		teta2d = atof(sz);
		sprintf(sz, "%.4lf\n", teta3d); 
		teta3d = atof(sz);*/


		//dirCin(teta1d, teta2d, teta3d, 0);
			
		res.teta1 = teta1d;	
		res.teta2 = teta2d;
		res.teta3 = teta3d;


		res.reachable = true;

	}
	else{


		if(recursive){
			
			if(res.reachable == false)
				res = invKin(x+0.002, y, z, false);
			
			if(res.reachable == false)
				res = invKin(x-0.002, y, z, false);
			
			if(res.reachable == false)
				res = invKin(x, y+0.002, z, false);
			
			if(res.reachable == false)
				res = invKin(x, y-0.002, z, false);
			
			if(res.reachable == false)
				res = invKin(x, y, z+0.002, false);
			
			if(res.reachable == false)
				res = invKin(x, y, z-0.002, false);

		}

	
		
	}
	
	
	if(res.reachable == false && recursive == true)
			std::cout << "Grasping point unreachable!" << std::endl;

	/*if(recursive == true)
		std::cout << "\n\nteta1: " << res.teta1 << "\nteta2: " << res.teta2 << "\nteta3: " << res.teta3 << std::endl;
	*/
	return res;
}



invKinResult invKin(float x, float y, float z){

	return invKin(x, y, z, true);
	
}
